#include "Relocalisation.h"
#include <glog/logging.h>
#include "Algorithm.hpp"
using namespace google;
namespace markerAR
{
    
extern Map* map;
extern Ptr<ORB> orb;
extern Ptr<cv::DescriptorMatcher> binaryMatcher;
extern Ptr<ORB> myorb;
extern Marker* nowMarker;
extern Ptr<IntrinsicMatrix> K;
extern Ptr<Keyframe> last;
extern Ptr<Tracking> track;
extern vector<shared_ptr<Marker>> allMarkers;
extern std::mutex mutex_keyframe_global;




//compute ssd of two images
//1. The intensity of images must be first minor the mean intensity(Approximation of light intensity),so the data type of Mat must be 16S
//2.
int64 Relocalisation::computeSSD(const Mat&a, const Mat&lowb)
{
    CV_Assert(a.type() == CV_8U);
    CV_Assert(lowb.type() == CV_16S);
    //#1: convert the mat type to short
    //#2: substract mean intenstiy value from all pixel intensities.
    Mat lowa;
    Mat ga, gb;
    CvSize ss = a.size();
    ss.height = ss.height;
    ss.width = ss.width;
    resize(a, lowa, lowb.size());
    lowa.convertTo(lowa, CV_16S);
    CV_Assert(lowa.size() == lowb.size());
    if(lowa.channels() == 1)
    {
        auto m = mean(lowa);
        for (int i=0; i<lowa.rows; ++i)
        {
            short * p = lowa.ptr<short>(i);
            for(int j=0;j<lowa.cols;++j)
            {
                p[j] -= m(0);
            }
        }
        
    }
    
    
    short * pa = (short*)lowa.data;
    short * pb = (short*)lowb.data;
    
    int length = lowa.rows*lowa.cols*lowa.channels();
    int64 ssd = 0;
    for (size_t i = 0; i < length; ++i)
    {
        ssd += (pa[i] - pb[i])*(pa[i] - pb[i]);
    }
    return ssd;
    
}
    
    
    bool Relocalisation::tryRelocalisationByBoW(const Mat&img,double timeStamp)
    {
        bg = true;
        Mat gray;
        if (img.channels() != 1)
        {
            cvtColor(img, gray, CV_BGR2GRAY);
        }
        else
        {
            gray = img.clone();
        }
        std::vector<cv::KeyPoint> kpts;
        cv::Mat desp;
        orb->operator()(img, Mat(), kpts,desp);
        auto result = bow.query(kpts, desp);
#define NDEBUG
#ifndef NDEBUG
        Mat debug;
        drawKeypoints(img, kpts, debug);
        imshow("kpts", debug);
        cvWaitKey();
#endif
        if (result.size()==0) {
            LOG(WARNING)<<"Relocalisation fail!No marker detected!"<<endl;
            bg = false;
            return false;
        }
        
        auto nowMarker = map->getMarker(result[0].id);
        Mat warp;
        warpPerspective(gray, warp, result[0].Htq, gray.size());

        orb->operator()(warp, Mat(), kpts, desp);
#ifndef NDEBUG
        drawKeypoints(warp, kpts, debug);
        imshow("warp", debug);
        imshow("marker", nowMarker->img);
        cvWaitKey();
#endif
        std::vector<vector<cv::DMatch>> dmatches;
        binaryMatcher->knnMatch(desp, nowMarker->orbStructure->desp, dmatches,2);
        std::vector<int> idx;
        GetMatch::GetInliersRadius(kpts, nowMarker->orbStructure->kpts, dmatches, idx, 0.6, 90, 10*10);
        if (idx.size()<20) {
            LOG(WARNING)<<"Relocalisation fail!No enough Correspondences"<<endl;
            bg = false;
            return false;
        }
        std::vector<cv::DMatch> dmatch;
        GetMatch::GetDMatch(dmatches, idx, dmatch);
        
        vector<cv::Point2f> now2d2;
        vector<Point3f> marker3D;
        GetMatch::prepare3D_2D(nowMarker->orbStructure->p3d, kpts, dmatch, marker3D, now2d2);
        transformPointsInverse(now2d2, result[0].Htq);
        cv::Matx33d R;
        cv::Matx31d t;
        Solve3D::getCameraRT(K->K_, marker3D, now2d2, R, t, false);
        
        Pose C;
        C.R = Converter::CV2Eigen(R);
        C.t = Converter::CV2Eigen(t);
        cout<<C.t<<endl;
        
        track->init(C,nowMarker,nowMarker->keyframe.get(),Converter::CV2Eigen(result[0].Htq).inverse(),timeStamp);
        bg =false;
        LOG(WARNING)<<"Relocalisation Success"<<endl;
        
        return true;
    }
    



//TODO: complete the tryRelocaliztion function
/*
 *	At first,We try to compute SSD value for all keyframes to decide the candidate frame.
 *	Then, we attempt to compute the H matrix from nowimg to the Marker img
 *	In the end, we warp the nowimg to the Marker,and use matching keypoint to calculate the H matrix.
 */
bool Relocalisation::tryRelocalisation(const Mat&img,double timeStamp)
{
    LOG(INFO)<<"Try Relocalisation"<<endl;
    
    bg = true;
    Mat gray;
    const Marker* nowMarker;
    if (img.channels() != 1)
    {
        cvtColor(img, gray, CV_BGR2GRAY);
    }
    else
    {
        gray = img.clone();
    }
    int64 minM = std::numeric_limits<int64>::max();
    int64 minK = minM;
    int minIdxM = -1,minIdxK = -1;
    for (int i=0; i<map->getMarkerSize(); ++i) {
        int64 ssd = computeSSD(gray, map->getMarker(i)->keyframe->m_smallBlurImage);
        if (ssd<minM || minM == -1)
        {
            minM = ssd;
            minIdxM = i;
        }
    }
    for (int i = 0; i<map->getKeyFrameSize(); ++i)
    {
        int64 ssd = computeSSD(gray, map->getKeyFrame(i)->m_smallBlurImage);
        if (ssd<minK || minK == -1)
        {
            minK = ssd;
            minIdxK = i;
        }
    }
    if (minIdxM<0&&minIdxK<0) {
        LOG(ERROR)<<"Relocalisation Fail! No keyframes!"<<endl;
        bg=false;
        return false;
    }
    Keyframe* kf;
    if (minM<=minK) {
        kf = map->getMarker(minIdxM)->keyframe.get();
    }
    else
        kf = map->getKeyFrame(minIdxK);
    int minIdx = minM <= minK ? minIdxM : minIdxK;
    int64 min =std::min(minM, minK);
    nowMarker = kf->markers[0];

//    //check if the marker changes
//    int markerID = kf->mainMarkerID;
//    if(nowMarker != allMarkers[markerID].get())
//    {
//        LOG(WARNING)<<"Marker Changed!Now MarkerID:"<<markerID<<endl;
//        //do some clean
//        mutex_keyframe_global.lock();
//        allKeyframes.clear();
//        for (int i=0; i<allMarkers.size(); ++i) {
//            allKeyframes.push_back(allMarkers[i]->keyframe);
//        }
//        mutex_keyframe_global.unlock();
//        minIdx = markerID;
//        nowMarker = allMarkers[minIdx].get();
//    }
    
    LOG(INFO)<<"Relocalisation candidate KeyframeID"<<minIdx<<endl;
    LOG(INFO)<<"Relocalisation SSD Error:"<<min<<endl;
    Keyframe now;
    TickMeter tm2;
    now.img = gray;
    //detect and compute orb feature in nowimg
    myorb->detect(img, now.feature.kpts);
    
    myorb->compute(img, now.feature.kpts, now.feature.desp);
    //match orb feature between nowimg and keyframe
    vector<vector<DMatch>> dmatches;
    binaryMatcher->knnMatch(now.feature.desp, kf->feature.desp, dmatches, 2);
    vector<int> idx;
    GetMatch::GetInliers(dmatches, idx, 0.6, 90);
    if (idx.size()<20)
    {
        LOG(WARNING)<<"Relocalisation fail!No enough Correspondences"<<endl;
        bg = false;
        return false;
    }
    vector<DMatch> dmatch;
    GetMatch::GetDMatch(dmatches, idx, dmatch);
    vector<Point2f> now2d2, kf2d2;
    GetMatch::preparePoint2D(now.feature.kpts, kf->feature.kpts, dmatch, now2d2, kf2d2);
//#define isDebug
#ifdef isDebug
    Mat out;
    drawMatches(img, now.feature.kpts, kf->img, kf->feature.kpts, dmatch, out);
    imshow("now-marker", out);
    cvWaitKey(0);
#endif
    //compute H matrix from nowimg to keyframe
    Mat mask;
    Matx33d Hkn = findHomography(now2d2, kf2d2, CV_RANSAC,3,mask);
    cout<<Hkn<<endl;
    int inlier = norm(mask, NORM_L1);
    //judge if it is a valid keyframe
    if (inlier<15)
    {
        LOG(WARNING)<<"Relocalisation fail!No enough Correspondences"<<endl;
        bg = false;
        return false;
    }
    LOG(ERROR)<<"Relocalisation: Homography between now and keyframe:"<<Hkn<<endl;
    //compute H from nowimg to marker
    Eigen::Matrix3d Hmn = kf->Hkm[0].inverse()*Converter::CV2Eigen(Hkn);
    LOG(ERROR)<<"Relocalisation: Homography between now and marker"<<endl<<Hmn<<endl;
    
    //warp nowimg to the Marker
    Mat warpimg;
    warpPerspective(gray, warpimg, Converter::Eigen2CV(Hmn),gray.size());
//#define isDebug
#ifdef isDebug
    imshow("warpimg", warpimg);
    imshow("markerimg", nowMarker->img);
    misc::warpMask(mask, gray.size(),Hmn.inverse());
    imshow("mask",mask);
    Mat marker2now;
    warpPerspective(nowMarker->img, marker2now, Converter::Eigen2CV(Hmn).inv(), gray.size());
    imshow("marker2now", marker2now);
    cvWaitKey(0);
    static int cnt = 0;
    char s[20];
    sprintf(s,"warp%d.jpg", cnt);
    imwrite(s, warpimg);
    cnt++;
#endif
    //detect warpimg orb feature and match with marker
    vector<KeyPoint> warpKpts;
    Mat warpDesp;
    orb->detect(warpimg, warpKpts);
    orb->compute(warpimg, warpKpts, warpDesp);
    binaryMatcher->knnMatch(warpDesp, nowMarker->orbStructure->desp, dmatches, 2);
    GetMatch::GetInliersRadius(warpKpts, nowMarker->orbStructure->kpts, dmatches, idx, 0.6, 90, 10*10);
    GetMatch::GetDMatch(dmatches, idx, dmatch);
    //prepare 3D and 2D pairs,then compute camera Pose
    vector<Point3f> marker3D;
    GetMatch::prepare3D_2D(nowMarker->orbStructure->p3d, warpKpts, dmatch, marker3D, now2d2);
    transformPointsInverse(now2d2, Converter::Eigen2CV(Hmn));
    
    if(dmatch.size()<10)
    {
        LOG(WARNING)<<"Relocalisation fail!No enough Correspondences"<<endl;
        bg = false;
        return false;
    }
#ifdef isDebug
    Mat out;
    drawPoints(img, now2d2, out);
    imshow("nowKpts", out);
    cvWaitKey(30);
#endif
    Matx33d R;
    Matx31d t;
    //	static bool isFirst = false;
    //	Solve3D::Homography_Decomposition(K->K, now.H, R, t);
    //	cout << R << t << endl;
    
    Solve3D::getCameraRT(K->K_, marker3D, now2d2, R, t, false);
//    cout<<marker3D[0]<<endl;
    Pose C;
    C.R = Converter::CV2Eigen(R);
    C.t = Converter::CV2Eigen(t);
    //if (isFirst == false)
    //	isFirst = true;
//#ifdef isDebug
//    cout << Solve3D::computeReprojectedError(marker3D, now2d2, K->K, R, t) << endl;
//    static int i = 0;
//    i++;
//    if (i % 10 == 0)
//    {
//        Mat ones = Mat::ones(gray.size(), CV_8U);
//        warpPerspective(ones, mask, now.H.inv(), gray.size());
//        
//        Keyframe kf;
//        myorb->detect(gray, kf.feature.kpts, mask);
//        myorb->compute(gray, kf.feature.kpts, kf.feature.desp);
//        kf.H = now.H;
//        kf.img = gray.clone();
//        allKeyframes.push_back(kf);
//    }
//#endif
    bg = false;
    cout<<"t"<<C.t<<endl;
    track->init(C,nowMarker,kf,Hmn.inverse(),timeStamp);
    LOG(INFO)<<"Relocalisation success"<<endl;
    return true;
}

Relocalisation::Relocalisation()
{
    bg = false;
    //	orb = new ORB(500);
    //	binaryMatcher = DescriptorMatcher::create("BruteForce-Hamming");
    
}

Relocalisation::~Relocalisation()
{
}
}